// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"

int ret;
int m_run0 = 1;
pthread_t threadid;

// 3.自定义节点类；
class Control_Valves : public rclcpp::Node
{
public:
    Control_Valves() : Node("control_valves_node_cpp")
    {
        serial_state_pub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("big_valve_state", 10);
        can_state_pub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("valve_state", 10);
        endaction_pub = this->create_publisher<std_msgs::msg::Int32>("end_action",10);
        // 启动一个线程用于接收用户输入
        ret = pthread_create(&threadid, NULL, input_func, this);
    }

    // 输入处理函数
    static void *input_func(void *arg)
    {
        int serial_val_1 = 0;
        int serial_val_2 = 0;

        int can_val_1 = 0;
        int can_val_2 = 0;

        int end_act = 0;

   

        Control_Valves *node = static_cast<Control_Valves *>(arg);
        while (rclcpp::ok())
        {
            auto serial_data = std_msgs::msg::Int32MultiArray();
            auto can_data = std_msgs::msg::Int32MultiArray();
            auto endact_data = std_msgs::msg::Int32();

            serial_data.data.resize(4);
            can_data.data.resize(2);
            //
            std::cout << "(隔爆)Enter serial valve “1” state (0 or 1): ";
            std::cin >> serial_val_1;
            serial_data.data[0] = serial_val_1;
            std::cout << "(隔爆)Enter serial valve “2” state (0 or 1): ";
            std::cin >> serial_val_2;
            serial_data.data[1] = serial_val_2;
            std::cout << "(本安)Enter can valve “1”  state (0 ~ 4): ";
            std::cin >> can_val_1;
            serial_data.data[2] = can_val_1;
            std::cout << "(本安)Enter can valve “2”  state (0 ~ 4): ";
            std::cin >> can_val_2;
            serial_data.data[3] = can_val_2;
            node->serial_state_pub_->publish(serial_data);
            std::cout<<""<<std::endl;
            //
            // 发布
            node->endaction_pub->publish(endact_data);
        }

        return nullptr;
    }

    ~Control_Valves()
    {
        pthread_cancel(threadid); // 强制终止线程
        pthread_join(threadid, NULL);
    }

private:
    // 发布阀门状态
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr serial_state_pub_;
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr can_state_pub_;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr endaction_pub;

};

int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    // 4.调用spain函数，并传入节点对象指针；
    rclcpp::spin(std::make_shared<Control_Valves>());
    // 5.资源释放
    rclcpp::shutdown();
    return 0;
}
